#include <stdio.h>
#include <string.h>
#include "pico/stdlib.h"
#include "pico/binary_info.h"
#include "lib/UART/uart.h"
#include "lib/DJI-P36/P36.h"
#include "mpu6050.h"
#include "inv_mpu.h"

float pitch, roll, yaw;        //欧拉角
short aacx, aacy, aacz;        //加速度传感器原始数据
short gyrox, gyroy, gyroz;    //陀螺仪原始数据

bool getmpu() {
    return true;
}

int main() {
    stdio_init_all();
    uartINIT();

    //MPU6050初始化
    MPU_Init();
    while (mpu_dmp_init()) {
        printf("MPU6050 Error\n");
    }

    //电机初始化
    p36_init();
    unsigned long id;
    uint8_t ext;
    uint8_t rtrBit;
    uint8_t len;
    uint8_t RX_buf[8];

    int16_t speed_set;

    //亮灯  初始化完成
    gpio_init(PICO_DEFAULT_LED_PIN);
    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
    gpio_put(PICO_DEFAULT_LED_PIN, 1);
    while (1) {
        if (!mpu_dmp_get_data(&pitch, &roll, &yaw)) {
//                    printf("pitch:%f roll:%f yaw:%f\n",pitch,roll,yaw);
            MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);    //得到陀螺仪数据
//            printf("%d,%d,%d\n", gyrox, gyroy, gyroz);
//            printf("%.2f,%.2f,%.2f\n", roll, pitch, yaw);
            speed_set = balance(pitch, gyroy);
            speed_set += velocity(p36_get_motor_RPM(MOTOR_1_ID),p36_get_motor_RPM(MOTOR_2_ID),pitch);
//            printf("set speed%d\n", speed_set);
            p36_set_motor_RPM(MOTOR_1_ID, speed_set);
            p36_set_motor_RPM(MOTOR_2_ID, speed_set);
        }
        tight_loop_contents();
    }
}

